/*
 * Jtag.cpp
 *
 *  Created on: Mar 6, 2020
 *      Author: Administrator
 */
#include <rtthread.h>
#include <cstdint>
#include <IJtagPort.h>
#include <Jtag.h>


Jtag Jtager;
void Jtag::Init()
{

}
bool Jtag::SetSpeed(uint32_t fkhz)
{
    delayNsNumber=1*1000/fkhz;
    return true;
}
inline uint8_t Jtag::TapTick(uint8_t tms,uint8_t tdi)
{
	uint8_t tdo=0;

    SetTms(tms);
    SetTdi(tdi);
    asm volatile("": : :"memory");
    Delay(delayNsNumber);
    asm volatile("": : :"memory");
    SetTck(1);
    asm volatile("": : :"memory");
    tdo=GetTdo();
    asm volatile("": : :"memory");
    Delay(delayNsNumber);
    asm volatile("": : :"memory");
    SetTck(0);
    asm volatile("": : :"memory");
    return tdo;
}
void Jtag::TapShift(uint32_t* oldp, uint32_t *newp, size_t len, uint8_t pre, size_t post)
{
    // Start state: Select-DR-Scan/Select-IR-Scan
  // End state: Run-Test/Idle

  int tdo;
  int tdi;
  size_t i;


  tap_assert(oldp);
  tap_assert(newp);
  tap_assert(len > 0);

  TapTick(0, 1); // Capture-DR/IR
  TapTick(0, 1); // Shift-DR/IR

  for(i = 0; i < pre; i++) {
      TapTick(0, 1);
  }

  for(i = 0; i < len; i++) {
    tdi = (newp[i / 32] >> (i % 32)) & 1;
    if(i == len - 1 && post == 0) {
      tdo = TapTick(1, tdi); // Exit1-DR/IR
    } else {
      tdo = TapTick(0, tdi);
    }
    if(tdo) {
        oldp[i / 32] |= 1 << (i % 32);
    } else {
        oldp[i / 32] &= ~(1 << (i % 32));
    }
  }

  for(i = 0; i < post; i++) {
      if(i == post - 1) {
          TapTick(1, 1); // Exit1-DR/IR
      } else {
          TapTick(0, 1);
      }
  }

  TapTick(1, 1); // Update-DR/IR
  TapTick(0, 1); // Run-Test/Idle
}

void Jtag::TapShiftDr(uint32_t* old_dr, uint32_t* new_dr, size_t dr_len)
{
    TapTick(1, 1); // Select-DR-Scan

    TapShift(old_dr, new_dr, dr_len, TARGET_CONFIG_TAP_DR_PRE, TARGET_CONFIG_TAP_DR_POST);
}
void Jtag::TapShiftIr(uint32_t* old_ir, uint32_t* new_ir, size_t ir_len)
{
    TapTick(1, 1); // Select-DR-Scan
    TapTick(1, 1); // Select-IR-Scan
    TapShift(old_ir, new_ir, ir_len, TARGET_CONFIG_TAP_IR_PRE, TARGET_CONFIG_TAP_IR_POST);
}
void Jtag::TapGoIdle(void)
{
  int i;
  for(i = 0; i < 6; i++){
    TapTick(1, 1);
  }
  TapTick(0, 1);
}
void Jtag::TapGoLogicReset()
{
  int i;
 for(i = 0; i < 6; i++){
	TapTick(1, 1);
  }
}
void Jtag::TapRun(uint32_t ticks)
{
    uint32_t i;

#ifdef TARGET_CONFIG_TAP_STATE_TRACE_EN
    tap_assert(tap_current_state == TAP_STATE_RUN_TEST_IDLE);
#endif // TARGET_CONFIG_TAP_STATE_TRACE_EN

    for(i = 0; i < ticks; i++) {
        TapTick(0, 1);
    }

#ifdef TARGET_CONFIG_TAP_STATE_TRACE_EN
    tap_assert(tap_current_state == TAP_STATE_RUN_TEST_IDLE);
#endif // TARGET_CONFIG_TAP_STATE_TRACE_EN    
}
